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Abstract 

A technique  for  generating  smooth  trajectories  of  a system  is  presented.  The  approach  is  to  find 
a dynamical  equation  with  the  desired  transient  behavior  and  use  it  as  the  basis  of  the  control  algo- 
rithm. It  is  shown  that  an  appropriate  dynamical  equation  can  be  obtained  by  varying  the  position 
and  velocity  gains  of  a proportional-derivative  control  loop  over  the  duration  of  the  movement. 
This  dynamical  equation  can  be  used  to  generate  trajectories  on-line  with  minimal  planning.  The 
resulting  manipulator  control  system  responds  to  path  errors  in  a more  reasonable  fashion  than  tra- 
ditional approaches  to  trajectory  control  since  explicit  replanning  is  not  required. 
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1.  Introduction 

Most  manipulator  control  systems  move  the  robotic  manipulator  by  periodically  updating  the 
attractor  of  a simple  control  loop.  (The  “attractor”  is  commonly  called  the  “reference”  or  “com- 
mand” signal,  however  the  language  of  [4]  will  be  adopted  here  to  provide  a more  geometric  inter- 
pretation.) The  attractor  typically  consists  of  joint  positions,  velocities,  and  accelerations,  but  may 
also  involve  force  or  position  in  Cartesian  coordinates  [1,2].  The  approach  of  periodically  updating 
the  attractor  suffers  from  the  fact  that  every  state  through  which  the  manipulator  is  to  pass  must  be 
planned  well  in  advance.  Thus,  with  planning,  the  manipulator  can  only  take  the  one  preplanned 
path  from  the  current  state  to  the  goal  state.  There  is  no  allowance  for  alternatives  based  on  the  sit- 
uation at  the  time  of  movement. 

Researchers  have  tried  many  variations  on  Proportional-Derivative  (PD)  control  to  improve  its 
usefulness  for  controlling  robotic  manipulators  [2,3].  This  paper,  in  Section  2,  reexamines  the  sim- 
ple PD  control  loop’s  transient  properties  when  used  without  preplanned  paths.  The  technique  of 
using  planned  trajectories  to  modify  the  behavior  of  the  simple  control  loop  is  discussed  in  Section 
3.  A new  approach  to  modifying  PD  controller  behavior  by  using  time-varying  gains  is  developed 
in  Section  4.  This  approach  allows  the  manipulator  to  move  toward  the  goal  state  along  a trajec- 
tory that  maintains  the  overall  smoothness  of  the  motion,  as  discussed  in  Section  5.  Section  6 dem- 
onstrates the  algorithm  on  a two-link  planar  manipulator  by  simulation. 

2.  Manipulator  Motion  Control 

Motion  control  of  robots  is  commonly  achieved  by  Proportional-Derivative  (PD)  servo  control 
loops  on  each  joint  [1].  The  form  of  this  control  is  given  by 

^act  = (1) 

where  x is  the  joint  position,  x is  the  joint  velocity,  and  Kp,  Ky  > 0 are  the  position  and  velocity 
gains.  The  control  output  of  the  PD  equation,  is  the  amount  of  torque  to  be  produced  by  the 

actuator.  (This  value  may  be  converted  into  a level  of  current  to  be  sent  to  the  motor.)  The  control 
equation  and  the  plant  (motor  and  mechanism)  form  a dynamical  system  with  a point  attractor  at 
the  joint  position  x^j.  This  means  that  the  system,  from  any  initial  position,  will  approach  a neigh- 
borhood of  the  point  x=  x^j,  eventually  reaching  the  neighborhood  and  staying  there  [4,5]. 

The  manner  in  which  the  attractor  is  approached  is  determined  by  the  transient  behavior  of  the 
dynamical  system.  The  transient  behavior  of  equation  (1)  is  less  than  ideal  for  manipulator  control. 
Figure  1 shows  simulations  of  the  position  and  velocity  profiles  of  a motion  generated  by  equation 
(1)  when  the  plant  is  a double  integrator,  i.e.  the  closed  loop  equation  is 

X = Kp{x^-x)  -KJ: 

The  velocity  profile  for  this  system  is  grossly  skewed  to  the  left,  showing  that  the  majority  of  the 
motion  occurs  early,  but  the  system  slows  down  and  approaches  the  attractor  very  slowly  at  the 
end.  Thus,  to  complete  a motion  within  a specified  time  interval,  a much  larger  than  necessary  peak 
velocity  must  be  generated. 

Khatib  [2]  presented  a modification  of  this  dynamical  system  that  would  limit  the  maximum 
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velocity.  For  a desired  maximum  velocity  of  the  modified  system  is  given  by 


(2) 


where 


V 


min 


V 


max 


This  system  still  has  a point  attractor  at  x=  x^j,  but  the  transient  behavior  has  been  modified.  The 
position  and  velocity  profiles  of  equation  (2)  are  shown  in  Figure  2.  Note  that  when  is  large, 
the  transient  behavior  of  equation  (2)  will  revert  to  that  of  equation  (1).  When  is  small  enough 

to  influence  the  behavior  of  the  system,  as  depicted  in  the  figure,  the  velocity  profile  takes  on  a 
somewhat  trapezoidal  shape  with  a significantly  smaller  peak  velocity  at  the  expense  of  a longer 
motion  duration. 

The  motions  generated  by  equations  (1)  and  (2)  both  suffer  from  a lack  of  “smoothness”.  Hog- 
an and  Flash  [6]  propose  the  use  of  the  mean  squared  magnitude  of  the  rate  of  change  of  accelera- 
tion (jerk)  as  a measure  of  the  smoothness  of  a movement. 

smoothness  = J|  ’Jc  \^dt  (3) 

0 

By  applying  a measure  such  as  this,  the  relative  smoothness  of  different  motions  can  be  compared. 
For  example,  for  the  two  motions  shown  in  Figures  1 and  2,  the  smoothness  measures  calculated 
from  (3)  are  1,817,214  and  1 1 1,838,  respectively,  over  the  interval  from  zero  to  two  seconds.  This 
implies  that  the  velocity-limited  motion  is  smoother,  in  effect  trading  movement  duration  for 
smoothness.  Both  of  the  motions  are  smooth  near  the  end  of  the  trajectory  and  “jerky”  at  the  be- 
ginning. The  amount  of  jerk  is  related  to  the  magnitude  of  the  gains.  For  the  example  motions 
shown  in  the  figures,  gains  of  Kp=  64  and  K^=  16  were  used. 


3.  Planned  Motion  Trajectories 

Large  initial  jerk  and  slow  final  movement  make  PD  controller  motions  less  than  ideal  for  gen- 
eration of  manipulator  trajectories.  When  a robotic  manipulator  is  moved  over  a large  distance,  the 
motion  should  be  controlled  so  that  the  velocity  peak  occurs  in  the  center  of  the  motion  and  is  not 
unreasonably  large.  To  achieve  this  with  a PD  controller,  planned  trajectories  are  normally  used  to 
guide  the  system’s  transient  motion.  Thus,  for  an  underlying  control  algorithm  of 

- ^)  + (4) 

a planned  sequence  of  desired  trajectory  points  (x^j,  x , x is  used  to  move  the  system  in  a desired 
motion.  By  periodically  updating  (x^j,  x ^ , x in  the  control  equation,  a desired  transient  behavior 
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can  be  achieved.  Thus,  a dynamical  system  with  undesirable  characteristics  is  made  to  behave  in  a 
desirable  manner  by  continuously  adjusting  its  attractor. 

One  type  of  planned  trajectory  used  for  manipulator  control  is  that  of  quintic  polynomials  in 
time  [1].  The  desired  motion  of  the  system  is  given  by 

xj^t)  = ^0  + ^1^+ ^2^^  ■‘"^5^ 

Xj^t)  = a^  + 2a2t  + ^a^t^  + Aa^t^  + 5a^t^ 

xj^t)  = 2^2  + 6^3  r + 12a4r^  + lOa^P 

for  0 < t < tf.  The  coefficients  a^  are  computed  to  satisfy  the  boundary  conditions  at  t=0  and  t=tf  for 
position,  velocity,  and  acceleration.  For  example,  suppose  the  boundary  conditions  are  x(0)=x^, 
X (0)  = X (0)  = 0,  and  Xjj(tf)=  x ^(tf)  = x ^(tf)  = 0.  Then  the  coefficients  for  the  quintic  polynomial 
trajectory  are  given  by  [1] 


^0  ” -^0 
= 0 

^2  = 0 
10^0 

ISjTq 


6xo 

“5  - "-T  (6) 

V 

Figure  3 shows  the  position  and  velocity  profiles  for  a quintic  polynomial  trajectory  with  a du- 
ration approximately  the  same  as  that  of  Figure  1.  A quintic  trajectory  will  be  very  smooth  accord- 
ing to  the  smoothness  measure  (3),  having  a value  of  814  for  the  motion  of  Figure  3.  The  symmetry 
of  the  velocity  profile  is  an  indication  of  the  smoothness  of  the  motion,  i.e.  the  motion  is  evenly 
distributed  over  time.  A double  integrator  plant  and  the  control  algorithm  (4)  were  used  to  produce 
the  motion  of  Figure  3.  The  control  gains  were  increased  to  Kp=  1024  and  K^=  64,  to  improve  the 

tracking  of  the  quintic  trajectory.  The  gains  must  be  increased  to  a level  appropriate  for  the  speed 
of  the  planned  motion  if  the  quintic  trajectory  is  to  be  tracked  accurately. 

Thus,  the  control  system  for  a planned  trajectory  will  involve  a high-gain  PD  controller.  Such 
a control  algorithm  can  create  problems.  For  example,  suppose  the  actual  initial  position  and  ve- 
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locity  of  the  arm  are  different  from  the  initial  conditions  used  in  computing  the  coefficients  of  the 
planned  trajectory.  This  situation  is  depicted  in  Figure  4.  Note  that  the  high-gain  PD  controller 
forces  the  system  quickly  back  to  the  planned  trajectory.  This  motion  is  not  smooth.  The  smooth- 
ness measure  for  the  ideal  quintic  (with  initial  conditions  x(0)=  2.0  and  x (0)  = 0.0)  is  88,  while  the 
smoothness  measure  for  the  quintic  with  incorrect  initial  conditions  of  x(0)=  2.2  and  x (0)=  0.5  is 
8,763,7 14.  Thus,  if  the  actual  initial  conditions  deviate  from  those  used  in  planning,  the  system  will 
not  be  able  to  produce  a smooth  movement.  This  problem  arises  because  the  system  has  the  un- 
smooth behavior  of  (1)  whenever  it  is  far  from  the  desired  attractor. 


4.  Dynamically  Generated  Trajectories 

Rather  than  settle  for  a fixed-gain  PD  controller  of  the  form 

'^act  - -^)  +Xd(0 

as  the  basis  of  manipulator  motion,  one  can  try  to  find  an  alternative  to  (1)  that  provides  better  tran- 
sient behavior.  One  such  alternative  is  the  dynamical  system  of  the  form 

= K^(t)x  + K^(t)  (x-x^)  (7) 


Here,  x is  the  plant  and  K^(t)  x + Kp(t)  (x  - x^)  represents  the  control  algorithm.  Such  a system 
can  be  devised  with  quintic  polynomial  trajectories  as  its  “natural”  transient  behavior. 

Consider  a dynamical  system  of  the  form 

(t)x  + K2  {t)x  + K^  (t)x  = 0 (8) 


that  is  to  have  a quintic  polynomial  trajectory  from  any  state  x=  Xq  to  the  final  state  x=  0.  (Assume 
that  the  desired  initial  and  final  velocities  and  accelerations  are  zero.)  From  (5)  and  (6),  we  have 


x(t)  = Xn  + 


-10^0  3 15j:o_4  , “6^0.5 


t -\- 


t + 
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-^Oxq  60xq  -30xq 
xit)  = — r— C + — + 


■f 
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'/ 


-60^0  180^0  -nOx^ 

x{t)  = — + 


/ 
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(9) 


Choose 

^1  (0  = bQ  + h^t 

K2  (t)  = Cq+  C-^t+  C2t^ 
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(10) 


^3  “ ^0^ 

Substituting  (9)  and  (10)  into  (8)  and  solving  for  the  unknown  coefficients  yields 


*0=8 


*'  = T 


Co  = 1 


Cl  = 


4r, 


C2  = 


^0  “ 


Thus,  the  dynamical  system 


-3 

2tj 

15 


2t 


f 


/o  ^ 2\ 

f 8 + -r--^r  1 


X = 


t-t 


’/ 


a:  + 


t-t 


■f 


(X-Xj) 


(11) 


is  obtained,  which  can  also  be  written  as 


1 — 12A,^  + 6X,  + 8 1 60A, 

^ = 7/ rn 


‘/ 


(12) 


where  X=  V t^.  There  is  a problem  with  the  denominators  of  gains  K^Ct)  and  Kp(t)  in  these  equa- 
tions, however.  The  function  (t  - tf)  is  zero  at  t=  tf.  The  gain  functions  grow  toward  minus  infinity 
as  t approaches  t^,  but  they  can  be  limited  to  some  reasonable  steady  state  values  and  held  there  for 
t > tf.  Therefore,  a system  valid  for  all  t > 0 is 


X = 


max(K^  (0 , -K^^)x  + maxiK^  (r) , -K^^)  {x- x^)  ,0<t  <t^ 


-K^^x-K^^  {x-x^). 


tf<t 


(13) 


where  Kp(t)  and  K^Ct)  are  given  in  equation  (11)  above. 
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Position  and  velocity  profiles  of  this  dynamical  system  (13)  in  approaching  the  point  attractor 
x=  Xjj  are  shown  in  Figure  5.  Here,  gain  limits  are  1024  and  64.  Note  that  the 

transient  behavior  is  a quintic  polynomial  trajectory,  (compare  with  Figure  3,)  and  therefore  has 
the  same  smoothness  properties  as  the  trajectories  described  in  the  previous  section.  This  system 
also  has  PD  controller  steady  state  behavior. 

There  are  many  ways  to  create  time-varying  gains  that  produce  quintic  polynomial  behavior, 
although  equation  (11)  may  be  the  simplest.  In  addition,  many  different  types  of  transient  responses 
can  be  created  using  the  basic  system  of  equation  (7).  Bullock  and  Grossberg  [7]  propose  a dynam- 
ical system  as  a model  of  human  motor  behavior.  This  system  can  be  written  in  the  form 


x=  ( ^-a)x-aG^ 


.2  A 


l^t^J 


(x-Xj) 


(14) 


Here,  a and  Gq  are  constants  which  determine  the  shape  of  the  transient  response  [7],  and  the  ve- 
locity gain  must  be  limited  near  t=0.  The  transient  response  of  this  system  can  be  symmetrical,  like 
the  quintic  polynomial  system,  but  for  many  movements  the  transient  response  looks  more  like 

at^ 

x(t)=e  than  a quintic  polynomial.  This  type  of  response  is  somewhere  in  between  the  perfectly 
symmetrical  quintic  polynomial  motion  and  the  highly  skewed  response  of  the  PD  controller  equa- 
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tion  (1).  A dynamical  system  providing  a response  with  the  shape  e'^^  is  given  by 

X = -3at^x-6at(x-x^)  (15) 

where  appropriate  limits  must  be  placed  on  the  gains  in  order  for  the  system  to  be  valid  for  all  t. 
The  position  and  velocity  profiles  of  (15)  with  a=  4 are  shown  in  Figure  6.  Compare  this  figure  with 
Figures  1 and  5.  The  smoothness  measure  for  the  trajectory  in  Figure  6 over  the  two- second  interval 
is  2025. 


5.  Smoothness  in  Dynamically  Generated  Trajectories 

The  dynamical  system  described  in  equation  (13)  has  quintic  polynomial  transient  behavior. 
This  system  will  be  referred  to  here  as  a dynamic  quintic  generator,  since  it  produces  quintic  poly- 
nomial-like trajectories  without  having  an  explicit  representation  of  the  quintic  polynomial.  The 
dynamic  quintic  generator  is  a PD  controller  that  uses  time-varying  gains  to  smooth  out  the  motion 
instead  of  periodically  updating  the  attractor.  This  means  that  the  system  is  not  forced  to  be  in  a 
certain  state  at  a certain  time,  but  rather  can  choose  an  arbitrary  state  that  is  appropriate  for  the  sit- 
uation. 

Note  that  the  equation  parameters  do  not  depend  on  the  initial  position  of  the  system,  unlike  the 
planned  trajectory  described  by  equations  (5)  and  (6).  The  system  (13)  only  depends  on  the  desired 
final  time  t^.  In  other  words,  only  the  duration  of  the  movement  need  be  specified  in  computing  the 

coefficients  of  Kp(t)  and  K^(t).  The  resulting  gains  will  provide  the  correct  transient  response  for 

movements  over  any  distance!  Figure  7 depicts  movements  of  three  different  distances  using  the 
functions  Kp(t)  and  Ky(t)  computed  for  t^  2 s.  Note  that  all  the  movements  finish  at  time  t=  2 s 

and  have  quintic  polynomial  profiles. 
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The  dynamic  quintic  generator  finishes  on  time  even  when  there  is  a non-zero  initial  velocity. 
A movement  from  the  initial  conditions  x(0)  = 2.2  rad,  x (0)  - 0.5  rad/s,  is  shown  in  Figure  8.  These 
initial  conditions  are  the  same  as  those  used  for  the  quintic  simulation  depicted  in  Figure  4.  The 
value  of  smoothness  measure  for  the  dynamic  quintic  generator  is  499  over  this  movement,  while 
it  is  8,763,7 14  for  the  planned  quintic  polynomial  movement.  The  dynamic  quintic  generator  main- 
tains its  smoothness  properties  even  when  there  is  variation  in  the  initial  conditions. 

Another  problem  area  for  planned  trajectories  is  switching  the  goal  point  during  the  middle  of 
the  movement.  Once  it  is  determined  that  a new  goal  point  is  to  be  attained,  the  planner  must  try  to 
predict  the  position,  velocity,  and  acceleration  of  the  system  at  some  time  t^.  Then  the  planner  must 

compute  a new  plan  for  the  system  using  the  initial  conditions  at  t^  and  start  the  execution  of  the 
new  plan  at  time  t^.  If  the  prediction  of  conditions  at  t^  is  correct  then  the  transition  to  the  new  tra- 
jectory should  be  smooth.  If  the  prediction  is  not  correct  and  the  low-level  controller  is  a high-gain 
PD  controller,  then  the  transition  will  definitely  be  jerky. 

Since  the  dynamic  quintic  generator  is  not  so  particular  about  initial  conditions,  the  transition 
to  a new  trajectory  can  be  done  while  maintaining  motion  smoothness.  The  switch  is  effected  by 
simply  changing  and  resetting  t to  zero.  If  a finish  time  for  the  entire  motion  other  than  t^+t^  is 

desired,  then  new  coefficients  of  the  gain  functions  can  be  determined.  Computing  new  coeffi- 
cients only  takes  four  multiplies  when  using  the  form  of  equation  (12).  Figure  9 depicts  a motion 
of  the  dynamic  quintic  generator  with  a switch  at  time  1.5  s.  The  original  goal  was  x^=  2 rad 

which  was  to  be  attained  2.0  s from  the  start  of  the  motion.  The  switched  motion  finishes  at  the  new 
goal  Xjj  = 1 rad  at  time  1.5  + 2.0  = 3.5  s.  The  motion  remains  smooth  even  with  the  switch  in  the 

middle.  The  value  of  the  smoothness  measure  for  the  entire  motion  is  288. 

6.  Simulation  of  Dynamic  Trajectory  Generation  on  Two-Link  Manipulator 

A simulation  of  a two-link  planar  manipulator  was  performed  to  demonstrate  the  applicability 
of  these  ideas  to  robot  manipulator  control.  The  dynamic  model  for  the  two-link  device  was  taken 
from  [1].  The  link  lengths  used  are  1.0  m for  link  1 and  0.8  m for  link  2.  The  link  masses  used  are 
5 kg  for  link  1 and  2 kg  for  link  2.  The  dynamic  simulation  included  all  inertia,  gravity,  centrifugal, 
and  Coriolis  terms,  but  no  frictional  disturbances. 

To  provide  an  accurate  comparison  between  the  PD  controller  with  planned  trajectory  and  the 
dynamic  quintic  generator  given  by  (13),  both  control  algorithms  included  gravity  and  inertia  com- 
pensation. The  PD  algorithm  is 

Vr  = -6)  +K,(Q^{t)  - 0)  + 0^ (r) ] + G (16) 

where  M is  the  inertia  matrix,  G is  the  gravity  vector,  and  the  vectors  0jj(t),  0(j(t),  and  0 ^(t)  are 
the  planned  trajectory  inputs  (which  are  updated  every  control  cycle.) 

The  dynamic  quintic  control  algorithm  is  given  by 

_(m  [max{K^  (r) , + rnaxlJC^  (t) , -K^)  (0  - 0^)  ] + G,  0 < r < 

act 

[ M[-K^9-K^‘^(Q-QJ]+G,  t^<t  (17) 
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where  M is  the  inertia  matrix,  G is  the  gravity  vector,  and  the  variable  gain  functions  Kp(t)  and 
Ky(t)  are  as  in  (1 1)  or  (12). 

Note  that  the  centrifugal  and  Coriolis  terms  of  the  dynamic  equation  are  not  compensated  in 
the  control  algorithms.  Thus,  these  terms  will  tend  to  act  as  a disturbance  on  the  control.  If  the  cen- 
trifugal and  Coriolis  terms  were  included  in  the  control  we  would  expect  the  systems  to  behave  as 
simulated  in  the  previous  sections. 

Figure  10  shows  the  positions  of  the  two  manipulator  joints  during  a motion  from  (0°,0°)  to 
(120°,45°)  in  one  second.  The  motions  of  the  PD  controller  with  the  planned  quintic  polynomial 
trajectory  and  the  dynamic  quintic  generator  have  comparable  smoothness.  For  joint  2,  the  PD  con- 
trol with  planning  has  a smoothness  measure  of  1649,  while  the  dynamic  quintic  generator’s 
smoothness  is  1718.  Also,  the  PD  controller  rejects  the  disturbances  caused  by  centrifugal  and 
Coriolis  effects.  This  is  due,  in  part,  to  the  acceleration  feedforward  term  of  (16).  Without  this  term, 
(16)  would  cause  the  joint  positions  to  overshoot  badly  and  not  reach  steady  state  within  one  sec- 
ond. In  general  for  the  two-link  simulations  it  is  found  that  the  dynamic  quintic  generator  does  not 
exactly  produce  a quintic  polynomial  trajectory,  but  maintains  its  smoothness  and  finishes  within 
the  specified  time  t^. 


7.  Conclusions 

A dynamical  system  that  corresponds  to  a PD  controller  with  time- varying  gains  has  been  de- 
scribed. This  system  produces  smooth  motion  to  point  attractors  which  is  different  from  the  natural 
motion  of  PD  controllers.  The  system  also  has  advantages  over  preplanned  trajectories  fed  into 
fixed-gain  PD  controllers,  namely,  more  smoothness  over  varied  initial  conditions.  The  system  is 
not  meant  to  replace  standard  PD  control  for  all  aspects  of  movement  — there  may  be  many  appli- 
cations where  tracking  plans  as  accurately  as  possible  is  the  best  solution,  e.g.  welding,  machining. 

There  is  some  indication  that  biological  motor  control  systems  formulate  dynamical  systems 
for  movement  based  on  the  task  [7,8].  It  is  not  difficult  to  believe  that  a dynamical  system  as  com- 
plex as  a mammalian  nervous  system  would  be  capable  of  emulating  a variety  of  dynamical  sys- 
tems, thereby  providing  motor  control  schemes  appropriate  to  the  task.  An  approach  to  robot  mo- 
tion which  has  as  its  most  fundamental  basis  fixed-gain  PD  control  makes  a limiting  assumption 
about  the  nature  of  robot  tasks.  The  uses  of  new  and  different  control  schemes  should  be  considered 
with  this  in  mind,  rather  than  evaluating  all  schemes  against  the  industrial  task.  Thus,  the  dynam- 
ical systems  examined  in  this  paper  represent  a first  step  in  exploring  the  more  complex  bases  of 
control  required  for  more  intelligent  behavior. 
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Eigure  1.  PD  Controller  Natural  Motion 


Figure  2.  PD  Controller  Velocity-Limited  Motion 
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Figure  3.  Quintic  Polynomial  Trajectory  Figure  4.  PD  Controller  and  Planned  Quin  tic  with 

Inexact  Initial  Conditions 


Figure  5.  Dynamic  Quintic  Generator  Motion 


of*. 

Figure  6.  Motion  of  e 


System 


11 


Figure  7.  Dynamic  Quintic  Generator  Motions 


Figures.  Dynamic  Quintic  Generator  with  Inexact 
Initial  Conditions 


Figure  9.  Quintic  Generator  with  Goal  Switch 


Figure  10.  Two-Link  Simulation  Comparison 
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